#include <opencv2/opencv.hpp>
#include <iostream>
#include <arm_neon.h>

void rgb_deinterleave_c(uint8_t *r, uint8_t *g, uint8_t *b, uint8_t *rgb, int len_color) {
    /*
     * Take the elements of "rgb" and store the individual colors "r", "g", and "b".
     */
    for (int i=0; i < len_color; i++) {
        r[i] = rgb[3*i];
        g[i] = rgb[3*i+1];
        b[i] = rgb[3*i+2];
    }
}

static void rgb_deinterleave_neon(uint8_t *r, uint8_t *g, uint8_t *b, uint8_t *rgb, int len_color) {
    /*
     * Take the elements of "rgb" and store the individual colors "r", "g", and "b"
     */
    int num8x16 = len_color / 16;
    uint8x16x3_t intlv_rgb;
    for (int i=0; i < num8x16; i++) {
        intlv_rgb = vld3q_u8(rgb+3*16*i);
        vst1q_u8(r+16*i, intlv_rgb.val[0]);
        vst1q_u8(g+16*i, intlv_rgb.val[1]);
        vst1q_u8(b+16*i, intlv_rgb.val[2]);
    }
}

double get_current_time()
{
    struct timeval tv;
    gettimeofday(&tv, NULL);

    return tv.tv_sec * 1000.0 + tv.tv_usec / 1000.0;
}

int main(void){
//     cv::Mat rgbSrc = cv::Mat::ones(1280, 720,CV_8UC3);
    cv::Mat rgbSrc = cv::imread("school.jpg");
    int rows = rgbSrc.rows;
    int cols = rgbSrc.cols;
    cv::Mat rDst(rows, cols, CV_8UC1, cv::Scalar(0));
    cv::Mat gDst(rows, cols, CV_8UC1, cv::Scalar(0));
    cv::Mat bDst(rows, cols, CV_8UC1, cv::Scalar(0));
    
    int loop_cnt = 100;
    
    {
        double time_min = DBL_MAX;
        double time_max = -DBL_MAX;
        double time_avg = 0;
        for(int i = 0; i < loop_cnt; ++i){
            double start = get_current_time();
            rgb_deinterleave_c(rDst.data, gDst.data, bDst.data, rgbSrc.data, cols * rows);
            double end = get_current_time();
            double time = end - start;
            time_min = std::min(time_min, time);
            time_max = std::max(time_max, time);
            time_avg += time;
        }
        time_avg = (time_avg - time_min - time_max) / (loop_cnt - 2);
        std ::cout << "  " << time_max << " " << time_min << " " << time_avg << std::endl;
    }
    
    cv::imwrite("case1.bmp", rDst);
    
    {
        double time_min = DBL_MAX;
        double time_max = -DBL_MAX;
        double time_avg = 0;
        for(int i = 0; i < loop_cnt; ++i){
            double start = get_current_time();
            rgb_deinterleave_neon(rDst.data, gDst.data, bDst.data, rgbSrc.data, cols * rows);
            double end = get_current_time();
            double time = end - start;
            time_min = std::min(time_min, time);
            time_max = std::max(time_max, time);
            time_avg += time;
        }
        time_avg = (time_avg - time_min - time_max) / (loop_cnt - 2);
        std ::cout << "  " << time_max << " " << time_min << " " << time_avg << std::endl;
    }
    
    cv::imwrite("case2.bmp", rDst);

    
    
//     std::cout << rgbSrc.size[0] << std::endl;
    return 0;
}
